#include "zed_actuator_calibration.h"


zed_actuator_calibration::zed_actuator_calibration(const char* left_img_dir,const char* right_img_dir)
{
    dirFindImg(left_img_dir, left_img_path_vec);
    dirFindImg(right_img_dir, right_img_path_vec);

    /**
    * @todo 添加执行机构计算的空间位置坐标，然后将空间中的执行机构坐标系下的目标点与空间中相机坐标系下的目标点对应起来，
    *       调用getRigidTrans3D函数计算出坐标变换矩阵
    */
}


/**
* @brief 计算空间两个坐标系间的变换矩阵
* @param[std::vector<cv::Point3d>] srcPoints 原坐标
* @param[std::vector<cv::Point3d>] dstPoints 目的坐标
*
* @return[TRIGID_TRANS_3D] 返回对应的坐标变换矩阵
*
*/
TRIGID_TRANS_3D zed_actuator_calibration::getRigidTrans3D(std::vector<cv::Point3d> srcPoints, std::vector<cv::Point3d> dstPoints)
{
    assert(srcPoints.size() == dstPoints.size());
    int pointsNum = srcPoints.size();

    TRIGID_TRANS_3D transform;
    double srcSumX = 0.0f;
    double srcSumY = 0.0f;
    double srcSumZ = 0.0f;

    double dstSumX = 0.0f;
    double dstSumY = 0.0f;
    double dstSumZ = 0.0f;

    for (int i = 0; i < pointsNum; ++ i)
    {
        srcSumX += srcPoints[i].x;
        srcSumY += srcPoints[i].y;
        srcSumZ += srcPoints[i].z;

        dstSumX += dstPoints[i].x;
        dstSumY += dstPoints[i].y;
        dstSumZ += dstPoints[i].z;
    }

    cv::Point3d centerSrc, centerDst;

    centerSrc.x = float(srcSumX / pointsNum);
    centerSrc.y = float(srcSumY / pointsNum);
    centerSrc.z = float(srcSumZ / pointsNum);

    centerDst.x = float(dstSumX / pointsNum);
    centerDst.y = float(dstSumY / pointsNum);
    centerDst.z = float(dstSumZ / pointsNum);

    cv::Mat srcMat(3, pointsNum, CV_32FC1);
    cv::Mat dstMat(3, pointsNum, CV_32FC1);

    float* srcDat = (float*)(srcMat.data);
    float* dstDat = (float*)(dstMat.data);
    for (int i = 0; i < pointsNum; ++ i)
    {
        srcDat[i] = srcPoints[i].x - centerSrc.x;
        srcDat[pointsNum + i] = srcPoints[i].y - centerSrc.y;
        srcDat[pointsNum * 2 + i] = srcPoints[i].z - centerSrc.z;

        dstDat[i] = dstPoints[i].x - centerDst.x;
        dstDat[pointsNum + i] = dstPoints[i].y - centerDst.y;
        dstDat[pointsNum * 2 + i] = dstPoints[i].z - centerDst.z;
    }

    cv::Mat matS = srcMat * dstMat.t();

    cv::Mat matU, matW, matV;
    cv::SVDecomp(matS, matW, matU, matV);

    cv::Mat matTemp = matU * matV;
    double det = cv::determinant(matTemp);

    double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
    cv::Mat matM(3, 3, CV_64FC1, datM);

    cv::Mat matR = matV.t() * matM * matU.t();

    memcpy(transform.matR, matR.data, sizeof(double) * 9);

    double* datR = (double*)(matR.data);
    transform.X = centerDst.x - (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);
    transform.Y = centerDst.y - (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);
    transform.Z = centerDst.z - (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);

    return transform;
}

void zed_actuator_calibration::dirFindImg(const char* dir_path, std::vector<std::string>& img_path_vec)
{
  DIR *dir;
  struct dirent *ptr;
  char base[1000];
  char *basePath = const_cast<char *>(dir_path);
  if ((dir=opendir(basePath)) == NULL)
  {
    std::perror("Open dir error...");
    exit(1);
  }

  while ((ptr=readdir(dir)) != NULL)
  {
    if(strcmp(ptr->d_name,".")==0 || strcmp(ptr->d_name,"..")==0)    ///current dir OR parrent dir
      continue;
    else if(ptr->d_type == 8)    ///file
    {
       std::string temp_path = basePath;
       std::string temp_name = ptr->d_name;
       std::string temp_img_path = temp_path + '/' + temp_name;
       img_path_vec.push_back(temp_img_path);
    }
    else if(ptr->d_type == 4)    ///dir
    {
      memset(base,'\0',sizeof(base));
      strcpy(base,basePath);
      strcat(base,"/");
      strcat(base,ptr->d_name);
      dirFindImg(base, img_path_vec);
    }
  }
  closedir(dir);
}

// Ubuntu上没有<io.h>文件，这个find image函数需要重写
/*
void zed_actuator_calibration::dirFindImg(const std::string dir_path, std::vector<std::string>& img_path_vec)
{
    long   hFile = 0;
    //文件信息
    struct _finddata_t fileinfo;//用来存储文件信息的结构体
    std::string p;
    if ((hFile = _findfirst(p.assign(dir_path).append("\\*").c_str(), &fileinfo)) != -1)  //第一次查找
    {
        do
        {
            if ((fileinfo.attrib &  _A_SUBDIR))  //如果查找到的是文件夹
            {
                if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)  //进入文件夹查找
                {
                    img_path_vec.push_back(p.assign(dir_path).append("\\").append(fileinfo.name));  // 存入目录
                    dirFindImg(p.assign(dir_path).append("\\").append(fileinfo.name), img_path_vec);
                }
            }
            else //如果查找到的不是是文件夹
            {
                img_path_vec.push_back(p.assign(dir_path).append("\\").append(fileinfo.name));
            }

        } while (_findnext(hFile, &fileinfo) == 0);

        _findclose(hFile); //结束查找
    }
}
*/



std::vector<cv::Point3d> zed_actuator_calibration::getWorldCoordinate(std::vector<std::string> left_img_path_vec, std::vector<std::string> right_img_path_vec)
{
    assert(left_img_path_vec.size() == right_img_path_vec.size());

    std::vector<cv::Point3d> camera_world_point;
    int num_img = left_img_path_vec.size();
    for(int i=0;i<num_img;i++)
    {
        auto left_img_path = left_img_path_vec.at(i);
        auto right_img_path = right_img_path_vec.at(i);

        const char* char_left_img_path = left_img_path.c_str();
        const char* char_right_img_path = right_img_path.c_str();
        stereo_camera_ranging stereo_camera(char_left_img_path, char_right_img_path);
        camera_world_point.push_back(stereo_camera.m_object_world_coordinate);
    }
    assert(num_img == (int) camera_world_point.size());
    return  camera_world_point;
}










